

#include "HeaderFiles.h"

STR_MOTOR_CTRL wheelCtrl[2];

STR_PID_HDL wheelSpdPIHdl[2];
TRAJ_Obj    SpeedRefRamp[2];

STR_PID_HDL angleSpdPIHd;

STR_MOP_CTRL mopCtrl[2];

uint16_t angleSpd_ctrl_cnt = 0;



uint16_t mop_ctrl_x1 = 0;
uint16_t mop_ctrl_x2 = 0;
uint16_t mop_ctrl_x3 = 0;
uint16_t mop_ctrl_x4 = 0;
uint16_t fan_speed_duty = 0;


void mop_ctrl_init(void);


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

float32_t abs_f(float32_t value)
{
	if(value >=0)
		return value;
	
	else
		return -value;
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
void motorctrl_init(void)
{
	
	wheelCtrl[WHEEL_LIFT].motor_num  = NUM_MOTOR_WHEEL_LEFT;
	wheelCtrl[WHEEL_RIGHT].motor_num = NUM_MOTOR_WHEEL_RIGHT;
			
	for(uint16_t i=0;i<2;i++)
	{
		wheelSpdPIHdl[i].kp = 20.0f;
		wheelSpdPIHdl[i].ki = 1.0f;
		
		wheelSpdPIHdl[i].outmin = -12;
		wheelSpdPIHdl[i].outmax = 12;
		
		wheelSpdPIHdl[i].robustpoint = 0.3f;
		
		SpeedRefRamp[i].intValue = 0;
		SpeedRefRamp[i].maxValue = WHEEL_SPEED_MAX;
		SpeedRefRamp[i].minValue = -WHEEL_SPEED_MAX;
		SpeedRefRamp[i].maxDelta = 0.02f;
		
		wheelCtrl[i].FSM = FSM_MOTOR_BREAK;
		wheelCtrl[i].stop_cnt = STOP_WAIT_TIME;
	}
	
	angleSpdPIHd.kp = 0.002f;
	angleSpdPIHd.ki = 0.0015f;
	angleSpdPIHd.kd = 0.001f;
	
	angleSpdPIHd.spearate = 200.0f;
	
	angleSpdPIHd.outmax = 1.2;
	angleSpdPIHd.outmin = -1.2;
	
	mop_ctrl_init();
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
void motorctrl_reset(STR_PID_HDL *piHdl)
{
	piHdl->ui = 0;
	piHdl->outvalue = 0;
	angleSpdPIHd.ui = 0;
	angleSpdPIHd.outvalue = 0;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
void motorctrl_pwmoutput(uint8_t motor_num, int16_t INT1_duty, int16_t INT2_duty)
{
	int16_t IN1_pwm_out = INT1_duty;
	int16_t IN2_pwm_out = INT2_duty;

	switch(motor_num)
	{
		case NUM_MOTOR_BRUSH_MAIN:
			
			if(INT2_duty == 1)
			{
				SET_MAIN_BRUSH_PWM_IN2();
				IN1_pwm_out = MATH_sat(IN1_pwm_out, PWM_PERIOD_REG, 0);
				timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_1, IN1_pwm_out);

			}
			else
			{
				RESET_MAIN_BRUSH_PWM_IN2();
				IN1_pwm_out = PWM_PERIOD_REG;
				timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_1, IN1_pwm_out);
			}
				
			break;
		
		case NUM_MOTOR_DUST_FAN:
			
			if(INT2_duty == 1)
			{
				#if 0
				SET_EDGE_BRUSH_PWM_IN2();
				#else
				SET_DUST_FAN_PWM_IN2();
				#endif
				IN1_pwm_out = MATH_sat(IN1_pwm_out, PWM_PERIOD_REG, 0);
				timer_channel_output_pulse_value_config(TIMER7, TIMER_CH_1, IN1_pwm_out);
								
			}
			else
			{
				#if 0
				RESET_EDGE_BRUSH_PWM_IN2();
				#else
				RESET_DUST_FAN_PWM_IN2();
				#endif
				IN1_pwm_out = PWM_PERIOD_REG;
				timer_channel_output_pulse_value_config(TIMER7, TIMER_CH_1, IN1_pwm_out);
			}
		
			break;
		
		
		
		case NUM_MOTOR_BRUSH_RIGHT:
			break;
		
		
		
		case NUM_MOTOR_BRUSH_LEFT:
			
			if(INT2_duty == 1)
			{
				#if 0
				SET_DUST_FAN_PWM_IN2();
				#else
				SET_EDGE_BRUSH_PWM_IN2();
				#endif
				IN1_pwm_out = MATH_sat(IN1_pwm_out, PWM_PERIOD_REG, 0);
				timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_0, IN1_pwm_out);
				
			}
			else
			{
				#if 0
				RESET_DUST_FAN_PWM_IN2();
				#else
				RESET_EDGE_BRUSH_PWM_IN2();
				#endif
				IN1_pwm_out = PWM_PERIOD_REG;
				timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_0, IN1_pwm_out);
			}
		
			break;
		
		
		
		case NUM_MOTOR_WHEEL_LEFT:
			
			timer_channel_output_pulse_value_config(TIMER0, TIMER_CH_3, IN1_pwm_out);
			timer_channel_output_pulse_value_config(TIMER0, TIMER_CH_0, IN2_pwm_out);			

			break;
		
		
		case NUM_MOTOR_WHEEL_RIGHT:
			
			timer_channel_output_pulse_value_config(TIMER7, TIMER_CH_0, IN1_pwm_out);
			timer_channel_output_pulse_value_config(TIMER0, TIMER_CH_1, IN2_pwm_out);						
		
			break;	
		
		
		// meidi mop motor run ctrl (need spin forward and backward)
		
		case NUM_MOTOR_MOP_RIGHT:	
		{
			
			if((IN1_pwm_out == 0) && (IN2_pwm_out == 0))
			{
				RESET_LEFT_MOP_PWM_IN2();
				IN1_pwm_out = PWM_PERIOD_REG;
				timer_channel_output_pulse_value_config(TIMER7, TIMER_CH_2, IN1_pwm_out);
			}
			else if(IN2_pwm_out != 0)
			{
				SET_LEFT_MOP_PWM_IN2();
				IN1_pwm_out = MATH_sat(IN1_pwm_out, PWM_PERIOD_REG, 0);
				timer_channel_output_pulse_value_config(TIMER7, TIMER_CH_2, IN1_pwm_out);
			}
			else
			{
				RESET_LEFT_MOP_PWM_IN2();
				IN1_pwm_out = MATH_sat((IN1_pwm_out), PWM_PERIOD_REG, 0);
				timer_channel_output_pulse_value_config(TIMER7, TIMER_CH_2, IN1_pwm_out);
			}
						
			break;
		}
		
		case NUM_MOTOR_MOP_LEFT:
		{
			
			if((IN1_pwm_out == 0) && (IN2_pwm_out == 0))
			{
				RESET_RIGHT_MOP_PWM_IN2();
				IN1_pwm_out = PWM_PERIOD_REG;
				timer_channel_output_pulse_value_config(TIMER0, TIMER_CH_2, IN1_pwm_out);
			}
			else if(IN2_pwm_out != 0)
			{
				SET_RIGHT_MOP_PWM_INT2();
				IN1_pwm_out = MATH_sat(IN1_pwm_out, PWM_PERIOD_REG, 0);
				timer_channel_output_pulse_value_config(TIMER0, TIMER_CH_2, IN1_pwm_out);							
			}
			else
			{
				RESET_RIGHT_MOP_PWM_IN2();
				IN1_pwm_out = MATH_sat((IN1_pwm_out), PWM_PERIOD_REG, 0);
				timer_channel_output_pulse_value_config(TIMER0, TIMER_CH_2, IN1_pwm_out);				
			}
									
			break;
		}
	}
}

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
void motorctrl_speed(STR_MOTOR_CTRL *ctrlHdl, STR_PID_HDL *piHdl)
{
	int16_t IN1_pwm_out = 0;
	int16_t IN2_pwm_out = 0;
	
	switch(ctrlHdl->FSM)
	{
		case FSM_MOTOR_IDLE :
			
			IN1_pwm_out = PWM_PERIOD_REG;
			IN2_pwm_out = PWM_PERIOD_REG;
		
			motorctrl_reset(piHdl);
		
		break;
		
		
		case FSM_MOTOR_RUN :
			
			#if 0
			piHdl->refvalue   = abs_f(ctrlHdl->speedref_rps);
			#else
			piHdl->refvalue = ctrlHdl->speedref_rps;
			#endif
			piHdl->fbackvaule = ctrlHdl->speedfb_rps;
		
			if(ctrlHdl->pid_mode == PI_ROBUST_PARALLEL)
			{
				pi_robust_run_parallel(piHdl);
			}
			else
			{
				pi_run_parallel(piHdl);
			}
		
			if(piHdl->outvalue >= 0)
			{				
				IN1_pwm_out = (MOTOR_IDLE_PWM_CAMP + (int16_t)((piHdl->outvalue / ctrlHdl->vdc_now) * PWM_PERIOD_REG));
				IN1_pwm_out = MATH_sat(IN1_pwm_out, PWM_PERIOD_REG, 0);
				
				IN2_pwm_out = 0;
				
			}
			else 
			{
				IN1_pwm_out = 0;
				IN2_pwm_out = (MOTOR_IDLE_PWM_CAMP + (int16_t)(( (0 - piHdl->outvalue) / ctrlHdl->vdc_now) * PWM_PERIOD_REG));
				IN2_pwm_out = MATH_sat(IN2_pwm_out, PWM_PERIOD_REG, 0);
			}

		break;
		
		
		case FSM_MOTOR_BREAK :
			
			IN1_pwm_out = 0;
			IN2_pwm_out = 0;
		
			motorctrl_reset(piHdl);
		
		break;
	
	}
	
	motorctrl_pwmoutput(ctrlHdl->motor_num,IN1_pwm_out, IN2_pwm_out);
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void angle_speed_ctrl(void)
{
	#if 0
	
	angleSpd_ctrl_cnt++;
	if((angleSpd_ctrl_cnt > 10) && (userHdl.ctrl.speedangle_dps != 0))
	{
		get_lsm6dsr_gyro_z(&userHdl.signal.angle_speed_dps);	
		
		angleSpdPIHd.refvalue = userHdl.ctrl.speedangle_dps;
		
		#if 0
		angleSpdPIHd.fbackvaule = userHdl.signal.angle_speed_dps;
		#else
		angleSpdPIHd.fbackvaule = userHdl.signal.angle_speed_dps_lpf;
		#endif
		
		pid_run_parallel(&angleSpdPIHd);
		
		userHdl.ctrl.speederro = angleSpdPIHd.outvalue;
				
		angleSpd_ctrl_cnt = 0;
	}
	else if(userHdl.ctrl.speedangle_dps == 0)
	{
		angleSpdPIHd.ui = 0;
		angleSpdPIHd.outvalue  = 0;
		userHdl.ctrl.speederro = 0;
		angleSpd_ctrl_cnt = 0;
	}
	
	#else
	
	userHdl.ctrl.speederro = (userHdl.ctrl.speedangle_rads * WHEEL_DIFFENCE_SPEED_SF);
	userHdl.ctrl.speederro = MATH_sat(userHdl.ctrl.speederro, WHEEL_SPEED_MAX, -WHEEL_SPEED_MAX);
	
	#endif	
}

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void calculate_wheel_speed(void)
{
	#if 0
	
	if((userHdl.signal.bumper[0] == 0) || (userHdl.signal.bumper[1] == 0))
	{		
		userHdl.ctrl.speedline_mps = MATH_sat(userHdl.ctrl.speedline_mps, 0, -WHEEL_SPEED_MAX);
	}		
		
	else if((userHdl.signal.cliff_flg[0] == 1) ||               \
		                (userHdl.signal.cliff_flg[1] == 1) ||   \
						(userHdl.signal.cliff_flg[2] == 1) ||   \
	                    (userHdl.signal.cliff_flg[3] == 1))
	{		
		userHdl.ctrl.speedline_mps = MATH_sat(userHdl.ctrl.speedline_mps, 0, -WHEEL_SPEED_MAX);
		
		#if 0
		userHdl.ctrl.speedRef_rps[WHEEL_LIFT]  = CLIFF_BACK_SPEED;
		userHdl.ctrl.speedRef_rps[WHEEL_RIGHT] = -CLIFF_BACK_SPEED;
		#elif 1
		wheelCtrl[WHEEL_LIFT].pid_mode  = PI_ROBUST_PARALLEL;
		wheelCtrl[WHEEL_RIGHT].pid_mode = PI_ROBUST_PARALLEL;
		#endif
	}		
	
	else if((userHdl.signal.cliff_flg[4] == 1) ||        \
		                (userHdl.signal.cliff_flg[5] == 1))
	{
		userHdl.ctrl.speedline_mps = MATH_sat(userHdl.ctrl.speedline_mps, WHEEL_SPEED_MAX, 0);
		
		#if 0
		userHdl.ctrl.speedRef_rps[WHEEL_LIFT]  = CLIFF_BACK_SPEED;
		userHdl.ctrl.speedRef_rps[WHEEL_RIGHT] = -CLIFF_BACK_SPEED;
		#elif 1
		wheelCtrl[WHEEL_LIFT].pid_mode  = PI_ROBUST_PARALLEL;
		wheelCtrl[WHEEL_RIGHT].pid_mode = PI_ROBUST_PARALLEL;
		#endif	
	}
	
	else
	{
		userHdl.ctrl.speedline_mps = MATH_sat(userHdl.ctrl.speedline_mps, WHEEL_SPEED_MAX, -WHEEL_SPEED_MAX);
		
		wheelCtrl[WHEEL_LIFT].pid_mode  = PI_PARALLEL;
		wheelCtrl[WHEEL_RIGHT].pid_mode = PI_PARALLEL;	
	}
	#else
	
	float32_t  front_speed_limit = WHEEL_SPEED_MAX;
	float32_t  back_speed_limit  = -WHEEL_SPEED_MAX;
	
	if((userHdl.signal.bumper[0] == 0) || (userHdl.signal.bumper[1] == 0))
	{		
		front_speed_limit = 0;
	}		
	
	if((userHdl.signal.cliff_flg[0] == 1) ||               \
		                (userHdl.signal.cliff_flg[1] == 1) ||   \
						(userHdl.signal.cliff_flg[2] == 1) ||   \
	                    (userHdl.signal.cliff_flg[3] == 1))
	{
		front_speed_limit = 0;
	}
	
	if((userHdl.signal.cliff_flg[4] == 1) ||        \
		                (userHdl.signal.cliff_flg[5] == 1))
	{	
		back_speed_limit = 0;
	}
	
	
	if((front_speed_limit == 0) || (back_speed_limit == 0))
	{
		wheelCtrl[WHEEL_LIFT].pid_mode  = PI_ROBUST_PARALLEL;
		wheelCtrl[WHEEL_RIGHT].pid_mode = PI_ROBUST_PARALLEL;		
		
		userHdl.ctrl.speedline_mps = MATH_sat(userHdl.ctrl.speedline_mps, front_speed_limit, back_speed_limit);
	}
	else
	{
		userHdl.ctrl.speedline_mps = MATH_sat(userHdl.ctrl.speedline_mps, WHEEL_SPEED_MAX, -WHEEL_SPEED_MAX);
		
		wheelCtrl[WHEEL_LIFT].pid_mode  = PI_PARALLEL;
		wheelCtrl[WHEEL_RIGHT].pid_mode = PI_PARALLEL;	
	}
		
	#endif
	
	
	#if 0  // motor speed ctrl ramp reference
	
	SpeedRefRamp[WHEEL_LIFT].targetValue = (userHdl.ctrl.speedline_mps - userHdl.ctrl.speederro);
	SpeedRefRamp[WHEEL_RIGHT].targetValue = -(userHdl.ctrl.speedline_mps + userHdl.ctrl.speederro);
	
	TRAJ_run(&SpeedRefRamp[WHEEL_LIFT]);
	TRAJ_run(&SpeedRefRamp[WHEEL_RIGHT]);
	
	
	userHdl.ctrl.speedRef_rps[WHEEL_LIFT] = SpeedRefRamp[WHEEL_LIFT].intValue;
	userHdl.ctrl.speedRef_rps[WHEEL_RIGHT] = SpeedRefRamp[WHEEL_RIGHT].intValue;
	
	#else  // motor speed ctrl no ramp reference
	
	userHdl.ctrl.speedRef_rps[WHEEL_LIFT]  = (userHdl.ctrl.speedline_mps - userHdl.ctrl.speederro);
	userHdl.ctrl.speedRef_rps[WHEEL_RIGHT] = -(userHdl.ctrl.speedline_mps + userHdl.ctrl.speederro);

	#endif
}

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void wheel_motor_ctrl(void)
{
	for(uint8_t num=0;num<2;num++)
	{
		wheelCtrl[num].vdc_now = userHdl.signal.real.vdc;
		
		#if 1
		if(userHdl.ctrl.speedRef_rps[num] == 0)
		#else
		if(SpeedRefRamp[num].targetValue == 0)
		#endif
		{
			wheelCtrl[num].stop_cnt++;
			
			if(wheelCtrl[num].stop_cnt > STOP_WAIT_TIME)
			{
				wheelCtrl[num].stop_cnt = STOP_WAIT_TIME;
				#if 0
				wheelCtrl[num].FSM = FSM_MOTOR_IDLE;
				#else
				wheelCtrl[num].FSM = FSM_MOTOR_BREAK;
				#endif
				wheelCtrl[num].speedref_rps = 0;
			}
			else
			{
				wheelCtrl[num].FSM = FSM_MOTOR_RUN;
				wheelCtrl[num].speedref_rps = userHdl.ctrl.speedRef_rps[num];		
			}
		}		
		else if (userHdl.ctrl.FSM == FSM_STATE_FAULT)
		{
			#if 0
			wheelCtrl[num].FSM = FSM_MOTOR_IDLE;
			#else
			wheelCtrl[num].FSM = FSM_MOTOR_BREAK;
			#endif
			wheelCtrl[num].speedref_rps = 0;
			
			wheelCtrl[num].stop_cnt = 0;
		}
		
		else
		{
			wheelCtrl[num].FSM = FSM_MOTOR_RUN;
			wheelCtrl[num].speedref_rps = userHdl.ctrl.speedRef_rps[num];
			
			wheelCtrl[num].stop_cnt = 0;
		}
		
		wheelCtrl[num].speedfb_rps  = userHdl.signal.wheel_speed_rps[num];
		motorctrl_speed(&wheelCtrl[num], &wheelSpdPIHdl[num]);
	}
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void motorctrl_wheel(void)
{
	angle_speed_ctrl();	
	
	calculate_wheel_speed();
		
	wheel_motor_ctrl();
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
void mop_ctrl_init(void)
{
	mopCtrl[0].motor_num = NUM_MOTOR_MOP_LEFT;
	mopCtrl[0].FSM = MOP_FSM_UNKNOWN;
	mopCtrl[0].up_run_duty = 1000;
	mopCtrl[0].down_run_duty = 500;
	
	mopCtrl[1].motor_num = NUM_MOTOR_MOP_RIGHT;
	mopCtrl[1].FSM = MOP_FSM_UNKNOWN;
	mopCtrl[1].up_run_duty = 1000;
	mopCtrl[1].down_run_duty = 500;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/






/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void mop_run_ctrl(STR_MOP_CTRL *hdl, uint8_t enable)
{
	hdl->enable = enable;
	
	switch(hdl->FSM)
	{
		case MOP_FSM_UNKNOWN:
				
			hdl->FSM = MOP_FSM_RUN_UP;
			hdl->holdtime_cnt = 0;		
		
		break;
		
		
		case MOP_FSM_STOP:
			
			if(hdl->enable == 1)
			{
				hdl->FSM = MOP_FSM_RUN_UP;
				hdl->holdtime_cnt = 0;
			}
			
			motorctrl_pwmoutput(hdl->motor_num, 0, 0);
			
		break;
		
		case MOP_FSM_RUN_UP:
			
			motorctrl_pwmoutput(hdl->motor_num, hdl->up_run_duty, 1);
			
			hdl->holdtime_cnt++;
		
			if(hdl->holdtime_cnt > MOP_UP_RUN_TIME)
			{
				hdl->holdtime_cnt = MOP_UP_RUN_TIME;
				
				if(hdl->enable == 0)
				{
					hdl->FSM = MOP_FSM_RUN_DOWN;
					hdl->holdtime_cnt = 0;
				}
			}
					
		break;
		
		
		case MOP_FSM_RUN_DOWN:

			motorctrl_pwmoutput(hdl->motor_num, hdl->down_run_duty, 0);
		
			hdl->holdtime_cnt++;
			
			if(hdl->holdtime_cnt > MOP_DOWN_RUN_TIME)
			{
				hdl->FSM = MOP_FSM_STOP;
				hdl->holdtime_cnt = 0;
			}
		
		break;
	
		case MOP_FSM_FAULT:
			
		break;
	
	}
}




/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void motorctrl_dc(void)
{	
	#if 1
	motorctrl_pwmoutput(NUM_MOTOR_BRUSH_MAIN, MAIN_BRUSH_PWM_DUTY, userHdl.ctrl.enable_mainbrush);
	motorctrl_pwmoutput(NUM_MOTOR_BRUSH_LEFT, EDGE_BRUSH_PWM_DUTY, userHdl.ctrl.enable_edgebrush);
	#endif
	
	#if 0
	motorctrl_pwmoutput(NUM_MOTOR_MOP_LEFT, mop_ctrl_x1, mop_ctrl_x2);
	motorctrl_pwmoutput(NUM_MOTOR_MOP_RIGHT, mop_ctrl_x1, mop_ctrl_x2);
	
	mop_ctrl_x3 = LEFT_MOP_POS_CHECK();
	mop_ctrl_x4 = RIGHT_MOP_POS_CHECK();
	#endif
	
	mop_run_ctrl(&mopCtrl[0], userHdl.ctrl.enable_leftmop);
	mop_run_ctrl(&mopCtrl[1], userHdl.ctrl.enable_rightmop);
	
	if(userHdl.ctrl.enable_dustfan == 1)
	{
		SET_FANMOTOR_CTRL();
	}
	else
	{
		RESET_FANMOTOR_CTRL();
	}
	
	motorctrl_pwmoutput(NUM_MOTOR_DUST_FAN  , DUST_FAN_PWM_DUTY  , userHdl.ctrl.enable_dustfan);
}







